/*
 * GPIO2_Key6.c
 *
 *  Created on: Mar 22, 2021
 *      Author: xrj
 */

#include "GPIO2_Key6.h"
#include "usbd_cdc_if.h"


//number call of DetKey6
uint32_t test_count=0;

/*
 * FloatTestLevel convert to two KeyState value,
 * please use below operate:
 * *(uint32_t*)k6s |= FTL2KS[x];     //assign to K1,K2
 * *(uint32_t*)k6s |= FTL2KS[x]<<8;  //assign to K5,K6
 */
uint32_t FTL2KS[5]={0b1010, 0b0110, 0b1001, 0b0101, 0b0000};

/*
 * @brief  init GPIO Pin of keyboard to float state.
 */
void GPIO2_Key6_Init()
{
	GPIO_Init5(A_Port, A_PinN);
	GPIO_Init5(B_Port, B_PinN);
}

/* @brief  read Pin use PinNumber
 * @param GPIOx     : x=A..G
 * @param GPIO_PinN : integer 0-15
 */
GPIO_PinState GPIO_ReadPinN(GPIO_TypeDef* GPIOx, uint32_t GPIO_PinN)
{
	assert_param(GPIO_PinN<16);
    if((GPIOx->IDR & (1<<GPIO_PinN))){
    	return GPIO_PIN_SET;
    }else{
    	return GPIO_PIN_RESET;
    }
}

/* @brief  set Pin to float input, count-- until to change state.
 *         if return 0, state not change during count.
 * @param GPIOx     : x=A..G
 * @param GPIO_PinN : integer 0-15
 * @param count     : init count
 * @retval : remain count
 */
uint32_t FloatCount(GPIO_TypeDef* GPIOx, uint32_t PinX, uint32_t count)
{
	GPIO_State5 last;
	last = GPIO_ReadPinN(GPIOx, PinX);
	GPIO_Set5(GPIOx, PinX, GPIO_Float);
	while(count && !GPIO_ReadPinN(GPIOx, PinX)^last){
		count--;  //TODO: use timer
	}
	return count;
}

/*
 * @brief  set GPIO to week mode(CNF=10 MODE=00), and flip bit on ODR,
 *         test time to IDR flip time.
 * @note   only for STM32F1
 * @param GPIOx     : x=A..G
 * @param GPIO_PinN : integer 0-15
 * @param count     : init count
 * @retval : remain count
 */
uint32_t InvCount(GPIO_TypeDef* GPIOx, uint32_t PinX, uint32_t count)
{
	uint32_t *bitband = (uint32_t*)BITBAND((uint32_t)&(GPIOx->IDR), PinX);
	GPIO_State5 nlast = !GPIO_ReadPinN(GPIOx, PinX);

	_Bool ODR_old = (GPIOx->ODR & 1<<PinX) != 0;
	uint32_t bsrr = 1 << (PinX + ODR_old*16);

	GPIO_SetCRLH(GPIOx, PinX, GPIO_Week_down);
    GPIOx->BSRR = bsrr;
    while(count && *bitband^nlast){
    	count--;  //TODO: use timer
    }
    return count;
}

/*
 * @brief ReadInput Level though pull up and down test.
 * @param GPIOx     : x=A..G
 * @param GPIO_PinN : integer 0-15
 * @retval : FloatTestLevel, result of read
 */
FloatTestLevel ReadInputTry(GPIO_TypeDef* GPIOx, uint32_t PinX)
{
	GPIO_Set5(GPIOx, PinX, GPIO_Week_down);
	Delay_MS(1);
	GPIO_PinState RPd = GPIO_ReadPinN(GPIOx, PinX);
	_Bool FCd = InvCount(GPIOx, PinX, InvCount_max)>InvCount_cut;

	GPIO_Set5(GPIOx, PinX, GPIO_Week_up);
	Delay_MS(1);
	GPIO_PinState RPu = GPIO_ReadPinN(GPIOx, PinX);
	_Bool FCu = InvCount(GPIOx, PinX, InvCount_max)>InvCount_cut;

	/* |   down  |   up    |
	 * | RPd FCd | RPu FCu |
	 * |---------|---------|
	 * |  0   1  |  1   1  | Lv_Float
	 * |  0   a  |  1   b  | Lv_Midd  (a && b=false)
	 * |  1   0  |  1   0  | Lv_High
	 * |  0   0  |  0   0  | Lv_Low
	 * |       other       | Lv_Error
	 */
	if(!RPd && RPu){
		if(FCd && FCu)
		    return Lv_Float;
	    else
		    return Lv_Midd;
	}
	if(!FCd && !FCu){
		if(RPd && RPu)
			return Lv_High;
		else if(!RPd && !RPu)
		    return Lv_Low;
	}return Lv_Error;
}

/*
 * @brief Merge two FloatTestLevel,
 *        used to detect K1,K2,K5,K6, remove most effects of K3,K4
 *
 * @note  one is error  -> Lv_Error
 *        00, x / x , 0 -> 0
 *        x ,11 / 11, x -> x
 *        01,10 / 10,01 -> Lv_Error
 *        above bins are State part(low 2bit) of FloatTestLevel
 */
FloatTestLevel Merge_FTLevel(FloatTestLevel x, FloatTestLevel y)
{
	if((x&Level_Error_Msk)||(y&Level_Error_Msk))
		return Lv_Error;
    if(x==0 || y==0)
    	return 0;
    if(x==Level_State_Msk)
    	return y;
    if(y==Level_State_Msk)
    	return x;
    return x;
}

uint32_t Set_ReadAB(GPIO_State5 A, GPIO_State5 B)
{
	if((A&Stro_Msk) && (B&Stro_Msk)){  //diode maybe damage, refuse test.
		return ((A&IsUp_IMsk)!=0)*2 + ((B&IsUp_IMsk)!=0); //return value by input value.
	}
	GPIO_Set5(A_Port, A_PinN, A);
	GPIO_Set5(B_Port, B_PinN, B);
	Delay_MS(1);
	return GPIO_ReadPinN(A_Port, A_PinN)*2 + GPIO_ReadPinN(B_Port, B_PinN);
}

/*
 * @brief Detect 6 Keys, try GPIO and analyze 6 Keys state.
 * @note  more than two key down in same time probably error happen.
 * @note  the result has noise.  //TODO: add filter
 */
void DetKey6(Key6Struct *k6s)
{
	*((uint32_t*)k6s) = 0;  //clear k6s struct
	GPIO_Set5(A_Port, A_PinN, GPIO_Float);
	GPIO_Set5(B_Port, B_PinN, GPIO_Float);

	GPIO_Set5(B_Port, B_PinN, GPIO_Stro_down);
	FloatTestLevel xD =  ReadInputTry(A_Port, A_PinN);
	GPIO_Set5(B_Port, B_PinN, GPIO_Stro_up);
	FloatTestLevel xU =  ReadInputTry(A_Port, A_PinN);
	GPIO_Set5(B_Port, B_PinN, GPIO_Float);
	FloatTestLevel xf =  ReadInputTry(A_Port, A_PinN);

	GPIO_Set5(A_Port, A_PinN, GPIO_Stro_down);
	FloatTestLevel Dx =  ReadInputTry(B_Port, B_PinN);
	GPIO_Set5(A_Port, A_PinN, GPIO_Stro_up);
	FloatTestLevel Ux =  ReadInputTry(B_Port, B_PinN);
	GPIO_Set5(A_Port, A_PinN, GPIO_Float);
	FloatTestLevel fx =  ReadInputTry(B_Port, B_PinN);

	//check K3
    if(xD!=Lv_Low || Ux!=Lv_High){
    	//K3 is not press
    	k6s->K3 |= Key_Up;
    }else if((xU==Lv_Low&&xf==Lv_Low && Dx==Lv_High&&fx==Lv_High) ||
    		 (xU==Lv_High&&(xf==Lv_High||xf==Lv_Midd) &&
			  Ux==Lv_High&&(fx==Lv_High||fx==Lv_Midd))){
        k6s->K3 |= Key_Unk;
    }else{
        k6s->K3 |= Key_Down;
    }

    //check K4
    if(xU!=Lv_High || Dx!=Lv_Low){
    	//K4 is not press
    	k6s->K4 |= Key_Up;
    }else if((xD==Lv_High&&xf==Lv_High && Ux==Lv_Low&&fx==Lv_Low) ||
   		     (xD==Lv_Low &&(xf==Lv_High||xf==Lv_Midd) &&
			  Ux==Lv_High&&(fx==Lv_High||fx==Lv_Midd))){
       k6s->K4 |= Key_Unk;
   }else{
	   k6s->K4 |= Key_Down;
   }

    //fill K3, K4
    //!K3 && !K4 ==> xD=xU=xf && Dx=Ux=fx
    // K3 &&  K4 ==> xD=Dx && xU=Ux && xf=fx
    if(xD==xU&&xU==xf && Dx==Ux&&Ux==fx){
    	if(xD!=Lv_Low || Ux!=Lv_High)
            k6s->K3 |= Key_Up;
    	if(xU!=Lv_High || Dx!=Lv_Low)
    	    k6s->K4 |= Key_Up;
    	if(xD!=Lv_Error)
    	    *(uint32_t*)k6s |= FTL2KS[xD];     //K1,K2
    	if(Dx!=Lv_Error)
    		*(uint32_t*)k6s |= FTL2KS[Dx]<<8;  //K5,K6
    }else if(k6s->K3 == Key_Up && k6s->K4 == Key_Up){
        k6s->K3 = Key_Error;
        k6s->K4 = Key_Error;
    }else if(k6s->K3 == Key_Up){
    	k6s->K4 |= Key_Down;
    }else if(k6s->K4 == Key_Up){
    	k6s->K3 |= Key_Down;
    }

    //check K1,K2  K5,K6
    if(k6s->K3 == Key_Down && k6s->K4 == Key_Down){
    	if(xf==Lv_Float && fx==Lv_Float){
    		*(uint32_t*)k6s |= 0b010100000101*Key_Up;
    	}else{
    		*(uint32_t*)k6s &= 0b000011110000;
    	}
    }else if(k6s->K3 == Key_Down){
    	*(uint32_t*)k6s |= FTL2KS[Merge_FTLevel(xU,xf)];     //K1,K2
    	*(uint32_t*)k6s |= FTL2KS[Merge_FTLevel(Dx,fx)]<<8;  //K5,K6
    }else if(k6s->K4 == Key_Down){
    	*(uint32_t*)k6s |= FTL2KS[Merge_FTLevel(xD,xf)];     //K1,K2
    	*(uint32_t*)k6s |= FTL2KS[Merge_FTLevel(Ux,fx)]<<8;  //K5,K6
    }

	test_count++;
	usb_printf("%4i | %i %i %i | %i %i %i ||=|| %i %i | %i %i | %i %i\n",
	     test_count , xD,xU,xf , Dx,Ux,fx,
		       k6s->K1, k6s->K2,
			   k6s->K3, k6s->K4,
			   k6s->K5, k6s->K6);
}
